classdef ObstacleAvoidance
    
    properties
        ship_length
        ship_width
        min_turn_radius
        safety_margin
        lookahead_distance
        hull_points
    end
    
    methods
        function obj = ObstacleAvoidance(ship_length, ship_width, min_turn_radius, safety_margin, lookahead_distance)
            % 初始化避障算法
            obj.ship_length = ship_length;
            obj.ship_width = ship_width;
            obj.min_turn_radius = min_turn_radius;
            
            if nargin < 4
                obj.safety_margin = 5.0; % 默认安全边界(m)
            else
                obj.safety_margin = safety_margin;
            end
            
            if nargin < 5
                obj.lookahead_distance = 50.0; % 默认前瞻距离(m)
            else
                obj.lookahead_distance = lookahead_distance;
            end
            
            % 船体轮廓点(局部坐标系)
            obj.hull_points = [
                ship_length/2,  ship_width/2;
                ship_length/2, -ship_width/2;
                -ship_length/2, -ship_width/2;
                -ship_length/2,  ship_width/2
            ];
        end
        
        function filtered = process_radar_data(obj, point_cloud)
            % 处理雷达点云数据，过滤无效点并转换到2D平面
            z_threshold = 2.0; % 忽略高于此高度的点
            z_values = point_cloud(:, 3);
            filtered = point_cloud(abs(z_values) < z_threshold, 1:2);
        end
        
        function risks = calculate_risk(obj, obstacles, current_speed, current_heading)
            % 计算每个障碍物的风险值
            if isempty(obstacles)
                risks = [];
                return;
            end
            
            % 将障碍物转换到船体坐标系(船头朝向为X轴正方向)
            rotation_matrix = [
                cos(current_heading),  sin(current_heading);
                -sin(current_heading), cos(current_heading)
            ];
            local_obstacles = obstacles * rotation_matrix;
            
            % 计算每个障碍物的风险
            distances = vecnorm(local_obstacles, 2, 2);
            angles = atan2(local_obstacles(:, 2), local_obstacles(:, 1));
            
            % 基于距离和角度的简单风险模型
            distance_risk = exp(-distances ./ (current_speed * 5.0 + 10.0));
            angle_risk = cos(angles/2); % 正前方的障碍物风险更高
            
            risks = distance_risk .* angle_risk;
        end
        
        function sector_safety = find_safe_directions(obj, obstacles, risks, current_heading, num_sectors)
            % 将周围空间划分为扇形区域，评估每个方向的安全性
            if nargin < 5
                num_sectors = 36; % 默认36个扇形
            end
            
            if isempty(obstacles)
                sector_safety = ones(1, num_sectors);
                return;
            end
            
            % 创建扇形区域
            sector_angles = linspace(0, 2*pi, num_sectors+1);
            sector_angles = sector_angles(1:end-1); % 移除最后一个重复点
            sector_safety = ones(1, num_sectors);
            
            % 将障碍物转换到船体坐标系
            rotation_matrix = [
                cos(current_heading),  sin(current_heading);
                -sin(current_heading), cos(current_heading)
            ];
            local_obstacles = obstacles * rotation_matrix;
            
            % 计算每个障碍物的角度和距离
            angles = atan2(local_obstacles(:, 2), local_obstacles(:, 1));
            distances = vecnorm(local_obstacles, 2, 2);
            
            % 评估每个扇形区域
            for i = 1:num_sectors
                angle = sector_angles(i);
                % 找到此扇形区域内的障碍物
                angle_diff = mod(angles - angle + pi, 2*pi) - pi;
                in_sector = abs(angle_diff) < (pi / num_sectors);
                
                if any(in_sector)
                    % 计算最危险的障碍物(考虑距离和风险)
                    sector_risks = risks(in_sector) ./ (distances(in_sector) + 1e-5);
                    max_risk = max(sector_risks);
                    sector_safety(i) = 1.0 / (1.0 + max_risk);
                end
            end
        end
        
        function target_point = select_target_point(obj, sector_safety, current_heading, current_speed)
            % 选择最安全的目标点
            num_sectors = length(sector_safety);
            sector_angles = linspace(0, 2*pi, num_sectors+1);
            sector_angles = sector_angles(1:end-1); % 移除最后一个重复点
            
            % 选择最佳几个方向(避免局部最优)
            [~, sorted_idx] = sort(sector_safety, 'descend');
            best_sectors = sorted_idx(1:min(3, num_sectors));
            best_angles = sector_angles(best_sectors);
            
            % 根据当前速度调整前瞻距离
            adaptive_distance = obj.lookahead_distance * (1 + current_speed / 5.0);
            
            % 计算候选目标点
            candidate_points = zeros(length(best_angles), 3); % x, y, safety
            for i = 1:length(best_angles)
                angle = best_angles(i);
                % 将局部角度转换为全局角度
                global_angle = angle + current_heading;
                
                % 计算目标点坐标
                x = adaptive_distance * cos(global_angle);
                y = adaptive_distance * sin(global_angle);
                safety = sector_safety(best_sectors(i));
                candidate_points(i, :) = [x, y, safety];
            end
            
            % 选择最佳目标点(考虑安全性和转向成本)
            best_score = -1;
            target_point = [0, 0]; % 默认值
            
            for i = 1:size(candidate_points, 1)
                x = candidate_points(i, 1);
                y = candidate_points(i, 2);
                safety = candidate_points(i, 3);
                
                % 计算转向成本(角度变化越小越好)
                target_angle = atan2(y, x);
                angle_diff = mod(target_angle - current_heading + pi, 2*pi) - pi;
                turn_cost = abs(angle_diff) / pi; % 归一化到[0,1]
                
                % 综合评分
                score = 0.7 * safety + 0.3 * (1 - turn_cost);
                
                if score > best_score
                    best_score = score;
                    target_point = [x, y];
                end
            end
        end
        
        function target_point = avoid_obstacles(obj, point_cloud, current_speed, current_heading)
            % 主避障函数
            % 处理雷达数据
            obstacles_2d = obj.process_radar_data(point_cloud);
            
            % 计算障碍物风险
            risks = obj.calculate_risk(obstacles_2d, current_speed, current_heading);
            
            % 评估各方向安全性
            sector_safety = obj.find_safe_directions(obstacles_2d, risks, current_heading);
            
            % 选择目标点
            target_point = obj.select_target_point(sector_safety, current_heading, current_speed);
        end
    end
end